package cn.edu.zjut;

import Jama.CholeskyDecomposition;
import Jama.Matrix;
import com.mathworks.toolbox.javabuilder.MWArray;
import com.mathworks.toolbox.javabuilder.MWClassID;
import com.mathworks.toolbox.javabuilder.MWNumericArray;
import lombok.Data;
import ukf_9link.UKFilter;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.stream.Collectors;

/**
 * @ClassName: FilterWindow
 * @Description: 滤波器窗口
 * @Author: 12071
 * @Date: 2021/8/11 13:51
 */
@Data
public class FilterWindow {
    private String bandMac;
    private List<NodePosition> positionHistory; //定位历史
    private Long timeSpan; // 采样时间间隔

    private final static int STATE_DIMENSION = 4;     //状态量维数 x,y,vx,vy
    private final static int MEASURE_DIMENSION = 2;     //观测量维度 x,y
    private final static Matrix Q = new Matrix(new double[][]{{2.5*0.000000000001,0},{0,2.5*0.000000000001}});    // 过程噪声方差
    private final static Matrix R = new Matrix(new double[][]{{0.0001*0.0001,0},{0,0.0001*0.0001}});    // 观测噪声方差
    private final static Matrix P_0 = new Matrix(new double[][]{{0.0001*0.0001,0,0,0},{0,0.0001*0.0001,0,0},{0,0,2.5*0.000000001,0},{0,0,0,2.5*0.000000001}});    //协方差矩阵初值


    // UT变换
    private final static double alpha = 1; //default, tunable
    private final static double kalpha = 1; //default, tunable
    private final static double beta = 1.0; //default, tunable
    private static double lambda = alpha * alpha * (STATE_DIMENSION + kalpha) - STATE_DIMENSION;   //数学研究表明最佳值一般为3-N
    private static double weight_0_m = lambda / (STATE_DIMENSION + lambda);
    private static double weight_0_c = lambda / (STATE_DIMENSION + lambda) + (1 - alpha * alpha + beta);
    private static double weight_i = 1 / (2 * (STATE_DIMENSION + lambda));
    private static double ns = Math.sqrt(STATE_DIMENSION + lambda);

    private Matrix B = new Matrix(STATE_DIMENSION, MEASURE_DIMENSION); // 过程噪声驱动矩阵(动态)
    private Matrix F = new Matrix(STATE_DIMENSION, STATE_DIMENSION); // 状态转移矩阵（动态）
    private Matrix P = new Matrix(new double[][]{{1000, 0, 0, 0}, {0, 1000, 0, 0}, {0, 0, 0.1 * 2.5, 0}, {0, 0, 0, 0.1 * 2.5}});    //协方差矩阵，初始化
    private Matrix K = new Matrix(STATE_DIMENSION, MEASURE_DIMENSION);   //Kalman增益系数

    /**
     * matlab参数
     */
    // 协方差矩阵初值
    private double[][] mP_0 = new double[][]{{0.0001*0.0001,0,0,0},{0,0.0001*0.0001,0,0},{0,0,0.0001*0.0001,0},{0,0,0,0.0001*0.0001}};
    // 协方差矩阵
    private double[][] mP = new double[][]{{0.0001*0.0001,0,0,0},{0,0.0001*0.0001,0,0},{0,0,0.0001*0.0001,0},{0,0,0,0.0001*0.0001}};
    // 过程噪声方差
    private double[][] mQ = new double[][]{{2.5*0.000000000001,0},{0,2.5*0.000000000001}};
    // 观测噪声方差
    private double[][] mR = new double[][]{{0.0001*0.0001,0},{0,0.0001*0.0001}};

    /**
     * 动态调整状态转移矩阵F和过程噪声驱动矩阵B（根据时间差）
     * 建议时间差为固定值，动态存在隐患
     * @param T
     */
    private void calBAndF(long T) {
        this.B = new Matrix(new double[][]{{Math.pow(T,2)/ 2, 0}, {0, Math.pow(T,2)/ 2}, {T, 0}, {0, T}});
        this.F = new Matrix(new double[][]{{1, 0, T, 0}, {0, 1, 0, T}, {0, 0, 1, 0}, {0, 0, 0, 1}});
    }

    public FilterWindow(String bandMac, Long timeSpan) {
        this.bandMac = bandMac;
        this.positionHistory = new ArrayList<>();
        this.timeSpan = timeSpan;
    }

    /**
     * 获取最新的位置
     * @return
     */
    public NodePosition getLastPosition() {
        int size = positionHistory.size();
        if (size <4) {
            return null;
        } else {
            return positionHistory.get(size - 1);
        }
    }

    /**
     * 直接添加位置信息
     * @param position
     */
    public void insertPosition(NodePosition position){
        this.positionHistory.add(position);
    }

    /**
     * 通过滤波添加位置信息
     * @param position
     */
    public void insertPositionWithUKF(NodePosition position){
        System.out.println("Before UKF:"+position);
        NodePosition fixedPosition = UKFilter(position);
        System.out.println("After UKF:"+fixedPosition);
        insertPosition(fixedPosition);
    }

    /**
     * 通过滤波添加位置信息（MATLAB）
     * @param position
     */
    public void insertPositionWithUKFMatlab(NodePosition position) {
//        System.out.println("Before UKF:"+position);
//        NodePosition fixedPosition = UKFilter(position);
        NodePosition lastPosition = getLastPosition();
        if (lastPosition != null) {
            MWNumericArray inputX = null;
            MWNumericArray inputZ = null;
            MWNumericArray inputP = null;
            MWNumericArray inputQ = null;
            MWNumericArray inputR = null;
            UKFilter ukf = null;
            try {
                double[][] dimsX = new double[][]{{lastPosition.getX()}, {lastPosition.getY()}, {0}, {0}};
                double[][] dimsZ = new double[][]{{position.getX()}, {position.getY()}};
                inputX = new MWNumericArray(dimsX, MWClassID.DOUBLE);
                inputZ = new MWNumericArray(dimsZ, MWClassID.DOUBLE);
                inputP = new MWNumericArray(mP, MWClassID.DOUBLE);
                inputQ = new MWNumericArray(mQ, MWClassID.DOUBLE);
                inputR = new MWNumericArray(mR, MWClassID.DOUBLE);

                ukf = new UKFilter();
                Object[] output = null;
                /**
                 * ukf_9link 使用matlab-deploytool导出的自定义函数
                 * @param i 返回的参数个数，这里返回两个参数
                 * @param Object... 传给matlab自定义函数的参数
                 *      X: 上一时刻的滤波结果 (4维)
                 *      P: 上一时刻滤波后的协方差
                 *      Z: 该时刻的观测值 (2维,经纬度坐标)
                 *      T: 采样时间间隔 (建议为固定值)
                 *      Q: 过程噪声均方差
                 *      R: 观测噪声均方差
                 * @return Object[]
                 *      X: 当前时刻的滤波结果
                 *      P: 更新后的协方差矩阵
                 */
                output = ukf.ukf_9link(2, inputX, inputP, inputZ, 5D, inputQ, inputR);
                System.out.println(Arrays.toString(output));
                MWNumericArray resultX = null;
                MWNumericArray resultP = null;
                resultX = (MWNumericArray) output[0];
                position.setX(resultX.getDouble(1));
                position.setY(resultX.getDouble(2));
                resultP = (MWNumericArray) output[1];
                mP = (double[][]) resultP.toDoubleArray();
            } catch (Exception e) {
                System.out.println(e);
            } finally {
                // 释放本地资源
                MWArray.disposeArray(inputX);
                MWArray.disposeArray(inputZ);
                MWArray.disposeArray(inputP);
                MWArray.disposeArray(inputQ);
                MWArray.disposeArray(inputR);
                if (ukf != null) {
                    ukf.dispose();
                }
            }
        }
        insertPosition(position);
    }

    /**
     * 通过滤波添加位置信息（移动平均法（一次））
     * @param position
     */
    public void insertPositionWithMovingMean(NodePosition position){
            position = movingMean(position);
        insertPosition(position);
    }

    /**
     * 根据历史位置估计速度
     * @param time
     * @return
     */
    public double[] estimateSpeed(long time) {
        int timeDelay = 5000 * 3; // 进行计算的时间范围
        List<NodePosition> history = positionHistory.stream().filter(position -> (time - position.getUpdateTime()< timeDelay)).collect(Collectors.toList());
        int size = history.size();
        double vx, vy;
        if (size < 2) {
            vx = 0;
            vy = 0;
        } else {
            vx = (history.get(size - 1).getX() - history.get(0).getX()) / 5;
            vy = (history.get(size - 1).getY() - history.get(0).getY()) / 5;
        }
        return new double[]{vx, vy};
    }

    /**
     * 无迹卡尔曼滤波过程
     */
    public NodePosition UKFilter(NodePosition measurePosition) {
        NodePosition lastPosition = getLastPosition();
        long T = lastPosition!=null?measurePosition.getUpdateTime() - lastPosition.getUpdateTime():0;
        if (T==0 || T>25000) { //初始数据或过时数据不予判断
            this.P = P_0;
            return measurePosition;
        }
        double[] speed = estimateSpeed(measurePosition.getUpdateTime());
        calBAndF(timeSpan);   // 动态调整转移矩阵
        // 1. 获取Sigma点
        System.out.println("P");
        P.print(P.getRowDimension(),P.getColumnDimension());
        P_0.print(P_0.getRowDimension(),P_0.getColumnDimension());
        CholeskyDecomposition chol = P_0.chol();
        Matrix cholesky = P.chol().getL().times(ns);
        Matrix sigmaPoint = new Matrix(STATE_DIMENSION, 2 * STATE_DIMENSION + 1);
        Matrix sigma_0 = new Matrix(new double[][]{{lastPosition.getX(), lastPosition.getY(), 0, 0}}).transpose();
        for (int i = 0; i < 2 * STATE_DIMENSION + 1; i++) {
            if (i == 0) {
                sigmaPoint.setMatrix(0, 3, i, i, sigma_0);
            } else if (i < STATE_DIMENSION+1) {
                sigmaPoint.setMatrix(0, 3, i, i, sigma_0.plus(cholesky.getMatrix(0, 3, i-1, i-1)));
            } else {
                sigmaPoint.setMatrix(0, 3, i, i, sigma_0.minus(cholesky.getMatrix(0, 3, (i - 1) % STATE_DIMENSION, (i - 1) % STATE_DIMENSION)));
            }
        }

        //2. 对Sigma点进行预测（sigmaPre =F*Sigma）
        Matrix Xpre = F.times(sigmaPoint);

        //3.利用2的预测结果计算均值和协方差
        // 均值
        Matrix Xpred_mean = Xpre.getMatrix(0, 3, 0, 0).times(weight_0_m);
        for (int i = 1; i < 2 * STATE_DIMENSION + 1; i++) {
            Xpred_mean = Xpred_mean.plus(Xpre.getMatrix(0, 3, i, i).times(weight_i));
        }
        //协方差
        Matrix Xpred_var = (Xpre.getMatrix(0, 3, 0, 0).minus(Xpred_mean)).times(Xpre.getMatrix(0, 3, 0, 0).minus(Xpred_mean).transpose()).times(weight_0_c);
        for (int i = 1; i < 2 * STATE_DIMENSION + 1; i++) {
            Xpred_var = Xpred_var.plus((Xpre.getMatrix(0, 3, 0, 0).minus(Xpred_mean)).times(Xpre.getMatrix(0, 3, 0, 0).minus(Xpred_mean).transpose()).times(weight_i));
        }
        Xpred_var = Xpred_var.plus(B.times(Q).times(B.transpose()));

        // 4. 根据预测值, 再一次使用UT变换，得到新的sigma点集

        // 5. 观测量预测
        // Z的均值
        Matrix Zpre = Xpre.getMatrix(0, 1, 0, 2 * STATE_DIMENSION);
//        Zpre.print(Zpre.getRowDimension(), Zpre.getColumnDimension());
        Matrix Zpre_mean = Zpre.getMatrix(0, 1, 0, 0).times(weight_0_m);
        for (int i = 1; i < 2 * STATE_DIMENSION + 1; i++) {
            Zpre_mean = Zpre_mean.plus(Zpre.getMatrix(0, 1, i, i).times(weight_i));
        }
        // Z的方差
        Matrix Zpre_var = (Zpre.getMatrix(0, 1, 0, 0).minus(Zpre_mean)).times(Zpre.getMatrix(0, 1, 0, 0).minus(Zpre_mean).transpose()).times(weight_0_c);
        for (int i = 1; i < 2 * STATE_DIMENSION + 1; i++) {
            Zpre_var = Zpre_var.plus((Zpre.getMatrix(0, 1, i, i).minus(Zpre_mean)).times(Zpre.getMatrix(0, 1, i, i).minus(Zpre_mean).transpose()).times(weight_i));
        }
        Zpre_var = Zpre_var.plus(R);

        // 状态与测量间的协方差
        Matrix pxz = (Xpre.getMatrix(0, 3, 0, 0).minus(Xpred_mean)).times(Zpre.getMatrix(0, 1, 0, 0).minus(Zpre_mean).transpose()).times(weight_0_c);
        for (int i = 1; i < 2 * STATE_DIMENSION + 1; i++) {
            pxz = pxz.plus((Xpre.getMatrix(0, 3, i, i).minus(Xpred_mean)).times(Zpre.getMatrix(0, 1, i, i).minus(Zpre_mean).transpose()).times(weight_i));
        }

        Matrix measure = new Matrix(new double[][]{{measurePosition.getX()},{measurePosition.getY()}});
        K = pxz.times(Zpre_var.inverse());

        Matrix UKFresult = Xpred_mean.plus(K.times(measure.minus(Zpre_mean)));
        P = Xpred_var.minus(K.times(Zpre_var).times(K.transpose()));
        measurePosition.setX(UKFresult.get(0,0));
        measurePosition.setY(UKFresult.get(1,0));
        return measurePosition;
    }

    /**
     * 移动平均法（一次）
     * @param position
     * @return
     */
    public NodePosition movingMean(NodePosition position){
        int historySize = getPositionHistory().size();
        if (historySize>0) {
            int count= Math.min(historySize, 5);
//            double xmean = positionHistory.get(historySize-1).getX()+(position.getXOri()-positionHistory.get(historySize-count).getXOri())/count;
//            double ymean = positionHistory.get(historySize-1).getY()+(position.getYOri()-positionHistory.get(historySize-count).getYOri())/count;
            double xmean=position.getXOri(),ymean=position.getYOri();
            for (int i = 1; i < count; i++) {
                xmean += positionHistory.get(historySize-i).getXOri();
                ymean += positionHistory.get(historySize-i).getYOri();
            }
            position.setX(xmean/count);
            position.setY(ymean/count);
        }
        return position;
    }
}
